import java.lang.Math;

public class CalcGPSSatPosition{
    
    // The Satellites position to be calculated on Geocentric coords
    private double X,Y,Z = 0.0;

    CalcGPSSatPosition() {
        
    }

    /**
      * ndata is the navigation data for the satellite position being calculated
      * epoch is the time at which we want a solution (seconds in gpsweek)
    */
    public void fromBroadCast(NavDataGPS nData, double epoch ) {
        // TODO: Ephemeris is valid for 2 hours before and after the Toe time stamp
        // confirm that this is valid reuqest

        GNSSConstants Const = new GNSSConstants();
 
        // Correct the satellite Time to system time
        // tSVCorr = t_svClock - delta_t_sv
        // delta_t_sv = a0 + a1(t - toc) + a2(t - toc)^2 
        // TODO: double check epoch is the right variable for 't' here
        // of if this should be an iterative calculation     
        double delta_tSV = nData.a0() + nData.a1() * (epoch - nData.toc());
        delta_tSV = delta_tSV + nData.a2() * ( (epoch - nData.toc()) * (epoch - nData.toc()) ) ;
        double tSV = nData.toc() - delta_tSV;
 
        // The time tk, elapsed since the reference epoch toe is
        double t = epoch - nData.Toe();

        // Semi - major axis
        double A = Math.pow(nData.sqrtA(),2);

        // Computed mean motion
        double n0 = Const.GM / Math.pow(A,3);
        n0 = Math.sqrt(n0);

        // Corrected mean motion
        double n = n0 + nData.dN();

        // Mean anomaly
        double M = nData.m0() + n * t; 

        //
        // Obtain the Satellite Coordinates
        // An iterative procedure , should only require 2 iterations
        // as the ececntricity of GPS satellites is very low e <= 0.001
        //
        // Kepler's equations of the eccentric anomaly
        //double E = Mk + e * math.sin(Ek);
        double E0 = M;
        double E = M + nData.ecc() * Math.sin(E0);

        // True Anomaly v
        double v = ( Math.sqrt( 1 - Math.pow(nData.ecc(),2) ) * Math.sin(E) ) / ( Math.cos(E) - nData.ecc() );
        v = Math.atan( v ) ;
        
        // Argument of perigee
        double omega = v + nData.omega();

        // Argument of perigee Correction
        double domega = nData.Cuc() * Math.cos( 2 * omega ) + nData.Cus() * Math.sin( 2 * omega );  

        // Radius Correction
        double dr = nData.Crc() * Math.cos( 2 * omega ) + nData.Crs() * Math.sin( 2 * omega );

        // Inclination Correction
        double di = nData.Cic() * Math.cos( 2 * omega ) + nData.CIS() * Math.sin( 2 * omega );

        // Corrected argument of perigee
        omega = omega + domega;

        // Corrected Radial Distance
        double r = A * ( 1 - nData.ecc() * Math.cos(E) ) + dr;

        // Corrected Inclination
        double i = nData.i0() + nData.idot() + di;

        // Position in the orbital plane
        double X_prime = r * Math.cos( omega ) ;
        double Y_prime = r * Math.sin( omega ) ;
        
        // Corrected Longitude of the ascending node accounting for the Earth's rotation rate
        double OMEGA = nData.OMEGA() + (nData.OMEGADot() - Const.omegaER ) * t - Const.omegaER * nData.Toe();

        // Satellite Coords in Geocentric System
        X = X_prime * Math.cos( OMEGA ) - Y_prime * Math.sin( OMEGA ) * Math.cos( i );
        Y = X_prime * Math.sin( OMEGA ) + Y_prime * Math.cos( OMEGA ) * Math.cos( i );
        Z = Y_prime * Math.sin( i );
    }
    
}
